/**
 * @file    main.c
 * @brief   H743-V2 飞控系统 - FreeRTOS + USB CDC
 * @author  H743-V2 飞控开发团队
 * @date    2024-11-08
 *
 * @details 硬件配置:
 *          - MCU: STM32H743VIH6 (Cortex-M7, 480MHz, 2MB Flash, 1MB RAM)
 *          - HSE: 8MHz 外部晶振
 *          - SYSCLK: 480MHz (PLL1: 8MHz / 1 * 120 / 2)
 *          - HCLK: 240MHz (SYSCLK / 2)
 *          - APB1/2/3/4: 120MHz (HCLK / 2)
 *          - USB时钟: 48MHz (PLL3: 8MHz / 2 * 48 / 4)
 *
 *          软件配置:
 *          - RTOS: FreeRTOS v10.4.6 (ARM_CM7/r0p1移植层)
 *          - 堆内存: 128KB 位于RAM_D1 (heap_4分配器)
 *          - USB: CDC虚拟串口
 *          - HAL时间基准: TIM6 (1ms tick, 独立于FreeRTOS SysTick)
 *
 *          核心特性:
 *          - LED心跳任务 (500ms间隔)
 *          - USB CDC状态报告任务 (1秒间隔)
 *          - 线程安全的USB传输 (互斥量保护)
 *          - 基于TIM6的HAL时间基准 (允许FreeRTOS独占SysTick)
 *          - HAL_GetTick()和HAL_Delay()完全正常工作
 */

#include "main.h"
#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"
#include "usbd_core.h"
#include "usbd_desc.h"
#include "usbd_cdc.h"
#include "usbd_cdc_if.h"
#include "ff.h"
#include "ff_gen_drv.h"
#include "bsp_sd_diskio.h"
#include "bsp_spi.h"
#include "bmi088.h"
#include "bmi270.h"
#include <string.h>
#include <stdio.h>
#include <stdarg.h>

/* USB设备句柄 */
USBD_HandleTypeDef hUsbDeviceFS;

/* TIM6句柄，用于HAL时间基准 */
TIM_HandleTypeDef htim6;

/* FreeRTOS句柄 */
static TaskHandle_t xLEDTaskHandle = NULL;
static TaskHandle_t xUSBTaskHandle = NULL;
static TaskHandle_t xIMUTaskHandle = NULL;
static SemaphoreHandle_t xUSBMutex = NULL;
static SemaphoreHandle_t xSPIMutex = NULL;  /* SPI总线互斥量 */

/* FreeRTOS堆内存 - 128KB位于RAM_D1 */
uint8_t __attribute__((section(".freertos_heap"))) ucHeap[configTOTAL_HEAP_SIZE];

/* FatFs相关 */
static FATFS SDFatFS;  /* SD卡文件系统对象 */
static char SDPath[4]; /* SD卡逻辑驱动器路径 */
static uint8_t sd_mounted = 0; /* SD卡挂载标志 */

/* USB接收缓冲区 */
#define USB_RX_BUFFER_SIZE 512
static char usb_rx_buffer[USB_RX_BUFFER_SIZE];
static uint16_t usb_rx_len = 0;

/* 命令处理缓冲区（用于从中断传递到任务） */
static char usb_cmd_buffer[USB_RX_BUFFER_SIZE];
static volatile uint8_t usb_cmd_ready = 0;

/* 私有函数原型 */
void SystemClock_Config(void);
static void GPIO_Init(void);
static void USB_Init(void);
static void SD_Init(void);
static void SPI_Init_All(void);
void Error_Handler(void);

/* FreeRTOS任务 */
static void LED_Task(void *pvParameters);
static void USB_Task(void *pvParameters);
static void IMU_Task(void *pvParameters);

/* PX4平台层测试 */
extern void freertos_api_run_tests(void);
extern void px4_sem_run_tests(void);
extern void hrt_call_run_tests(void);
extern void intrusive_queue_run_tests(void);
extern void intrusive_sorted_list_run_tests(void);

/* C++ LockGuard测试 */
#ifdef __cplusplus
extern "C" {
#endif
extern void test_lock_guard_cpp(void);
extern void test_blocking_list(void);
extern void test_blocking_queue(void);
extern void test_work_queue_manager(void);
extern void test_work_item(void);
extern void test_scheduled_work_item(void);
extern void test_work_item_single_shot(void);
extern void test_integration(void);
#ifdef __cplusplus
}
#endif

/* PX4平台层初始化 */
extern void hrt_init(void);

/* 调试输出函数 */
void debug_printf(const char *format, ...);

/* SD卡文件操作函数 */
static void SD_WriteFile(const char *filename, const char *content);
static void SD_ReadFile(const char *filename);
static void SD_ListFiles(void);
static void SD_DeleteFile(const char *filename);
static void SD_ShowInfo(void);

/* USB命令处理函数 */
static void USB_ProcessCommand(char *cmd);

/* 外部CDC发送函数 */
extern uint8_t CDC_Transmit_FS(uint8_t* Buf, uint16_t Len);

/**
 * @brief 覆盖HAL_InitTick函数，使用TIM6代替SysTick
 *
 * 这允许FreeRTOS独占SysTick，而HAL库使用TIM6作为时间基准。
 * 这是ST官方推荐的FreeRTOS集成方案。
 *
 * TIM6配置:
 * - 时钟源: APB1 × 2 = 240MHz (当APB1预分频器≠1时)
 * - 预分频器: 239 (240MHz / 240 = 1MHz)
 * - 周期: 999 (1MHz / 1000 = 1kHz = 1ms tick)
 * - 中断优先级: 15 (最低优先级，与FreeRTOS相同)
 *
 * @param TickPriority: Tick中断优先级
 * @retval HAL状态
 */
HAL_StatusTypeDef HAL_InitTick(uint32_t TickPriority)
{
    RCC_ClkInitTypeDef clkconfig;
    uint32_t uwTimclock = 0;
    uint32_t uwPrescalerValue = 0;
    uint32_t pFLatency;
    HAL_StatusTypeDef status;

    /* 使能TIM6时钟 */
    __HAL_RCC_TIM6_CLK_ENABLE();

    /* 获取时钟配置 */
    HAL_RCC_GetClockConfig(&clkconfig, &pFLatency);

    /* 获取APB1时钟频率 (TIM6位于APB1总线) */
    uwTimclock = HAL_RCC_GetPCLK1Freq();

    /* 重要: 当APB1预分频器≠1时，定时器时钟 = APB1 × 2
     * APB1 = 120MHz，但TIM6时钟 = 240MHz (因为D2PPRE1 = /2)
     * 所以需要乘以2 */
    if (clkconfig.APB1CLKDivider != RCC_APB1_DIV1)
    {
        uwTimclock *= 2;  /* 当预分频器≠1时，定时器时钟是APB1的2倍 */
    }

    /* 计算预分频器以获得1MHz计数时钟 */
    /* TIM6时钟 = 240MHz，所以预分频器 = (240MHz / 1MHz) - 1 = 239 */
    uwPrescalerValue = (uint32_t)((uwTimclock / 1000000U) - 1U);

    /* 初始化TIM6 */
    htim6.Instance = TIM6;
    htim6.Init.Period = (1000000U / 1000U) - 1U;  /* 1MHz / 1kHz - 1 = 999 */
    htim6.Init.Prescaler = uwPrescalerValue;
    htim6.Init.ClockDivision = 0;
    htim6.Init.CounterMode = TIM_COUNTERMODE_UP;
    htim6.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;

    status = HAL_TIM_Base_Init(&htim6);
    if (status != HAL_OK)
    {
        return status;
    }

    /* 配置TIM6中断优先级 */
    /* 使用最低优先级(15)以不干扰FreeRTOS */
    HAL_NVIC_SetPriority(TIM6_DAC_IRQn, TickPriority, 0);
    HAL_NVIC_EnableIRQ(TIM6_DAC_IRQn);

    /* 启动TIM6 */
    return HAL_TIM_Base_Start_IT(&htim6);
}

/**
 * @brief TIM6周期结束回调函数
 *
 * 当TIM6更新事件发生时，HAL_TIM_IRQHandler()会调用此函数。
 * 它递增HAL tick计数器(uwTick)，该计数器被HAL_GetTick()使用。
 *
 * @param htim: TIM句柄
 */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    if (htim->Instance == TIM6)
    {
        HAL_IncTick();  /* 递增HAL tick计数器 */
    }
}

/**
 * @brief  主程序
 */
int main(void)
{
    /* HAL库初始化 */
    HAL_Init();

    /* 配置系统时钟 */
    SystemClock_Config();

    /* 初始化GPIO */
    GPIO_Init();

    /* 初始化USB */
    USB_Init();

    /* 初始化SPI（仅GPIO和外设配置，不初始化传感器） */
    SPI_Init_All();

    /* 初始化PX4平台层 - HRT定时器 */
    hrt_init();

    /* 创建USB互斥量 */
    xUSBMutex = xSemaphoreCreateMutex();
    if (xUSBMutex == NULL)
    {
        Error_Handler();
    }

    /* 创建SPI互斥量 */
    xSPIMutex = xSemaphoreCreateMutex();
    if (xSPIMutex == NULL)
    {
        Error_Handler();
    }

    /* 创建LED任务 */
    if (xTaskCreate(LED_Task, "LED", 256, NULL, tskIDLE_PRIORITY + 1, &xLEDTaskHandle) != pdPASS)
    {
        Error_Handler();
    }

    /* 创建USB任务 */
    if (xTaskCreate(USB_Task, "USB", 1024, NULL, tskIDLE_PRIORITY + 2, &xUSBTaskHandle) != pdPASS)
    {
        Error_Handler();
    }

    /* 创建IMU任务 */
    if (xTaskCreate(IMU_Task, "IMU", 1024, NULL, tskIDLE_PRIORITY + 5, &xIMUTaskHandle) != pdPASS)
    {
        Error_Handler();
    }
    debug_printf("IMU task created successfully\r\n");

    /* 启动调度器 */
    debug_printf("Starting FreeRTOS scheduler...\r\n");
    vTaskStartScheduler();

    /* 永远不应该到达这里 */
    Error_Handler();
}

/**
 * @brief LED任务 - 心跳指示器
 */
static void LED_Task(void *pvParameters)
{
    (void)pvParameters;

    while (1)
    {
        /* 每500ms翻转绿色LED */
        HAL_GPIO_TogglePin(GPIOE, GPIO_PIN_2);
        vTaskDelay(pdMS_TO_TICKS(500));
    }
}

/**
 * @brief USB任务 - 通过USB CDC进行状态报告
 */
static void USB_Task(void *pvParameters)
{
    (void)pvParameters;
    uint32_t counter = 0;
    uint32_t hal_tick_start, hal_tick_end;
    TickType_t rtos_tick_start, rtos_tick_end;

    /* 等待USB枚举完成和主机驱动加载 */
    vTaskDelay(pdMS_TO_TICKS(5000));

    /* 打印启动横幅 */
    debug_printf("\r\n");
    debug_printf("========================================\r\n");
    debug_printf("  H743-V2 Flight Controller\r\n");
    debug_printf("  MCU: STM32H743VIH6\r\n");
    debug_printf("  SYSCLK: 480MHz, HCLK: 240MHz\r\n");
    debug_printf("  RTOS: FreeRTOS v10.4.6\r\n");
    debug_printf("  USB: CDC Virtual COM Port\r\n");
    debug_printf("  HAL Timebase: TIM6 (1ms tick)\r\n");
    debug_printf("========================================\r\n");
    debug_printf("\r\n");

    /* 初始化SD卡 */
    debug_printf("Initializing SD card...\r\n");
    SD_Init();
    debug_printf("\r\n");

    /* 运行PX4平台层单元测试 */
    debug_printf("Running PX4 Platform Unit Tests...\r\n");
    freertos_api_run_tests();  // 步骤0：FreeRTOS API兼容性验证
    px4_sem_run_tests();
    hrt_call_run_tests();
    intrusive_queue_run_tests();
    intrusive_sorted_list_run_tests();  // 步骤1：IntrusiveSortedList
    debug_printf("\r\n=== About to call C++ LockGuard tests ===\r\n");
    test_lock_guard_cpp();   // 步骤1.5：LockGuard (C++ RAII version)
    debug_printf("=== C++ LockGuard tests completed ===\r\n\r\n");
    test_blocking_list();    // 步骤2：BlockingList (thread-safe sorted list)
    test_blocking_queue();   // 步骤3：BlockingQueue (blocking queue)
    test_work_queue_manager(); // 步骤4：WorkQueueManager
    test_work_item();        // 步骤5：WorkItem
    test_scheduled_work_item(); // 步骤6：ScheduledWorkItem
    test_work_item_single_shot(); // 步骤7：WorkItemSingleShot
    test_integration();      // 步骤8：集成测试

    /* 测试HAL_GetTick() */
    hal_tick_start = HAL_GetTick();
    vTaskDelay(pdMS_TO_TICKS(1000));
    hal_tick_end = HAL_GetTick();
    debug_printf("HAL_GetTick() test: %lu ms elapsed (expected ~1000ms)\r\n",
                 hal_tick_end - hal_tick_start);

    /* 测试HAL_Delay() */
    debug_printf("Testing HAL_Delay(500)...\r\n");
    rtos_tick_start = xTaskGetTickCount();
    hal_tick_start = HAL_GetTick();
    HAL_Delay(500);
    hal_tick_end = HAL_GetTick();
    rtos_tick_end = xTaskGetTickCount();
    debug_printf("HAL_Delay(500) test: HAL=%lums, RTOS=%lums\r\n",
                 hal_tick_end - hal_tick_start,
                 rtos_tick_end - rtos_tick_start);
    debug_printf("\r\n");

    while (1)
    {
        /* 检查是否有命令需要处理 */
        if (usb_cmd_ready)
        {
            /* 处理命令 */
            USB_ProcessCommand(usb_cmd_buffer);

            /* 清除命令就绪标志 */
            usb_cmd_ready = 0;
        }

        /* 发送周期性状态（每3秒一次） */
        debug_printf("[%lu] System OK - Free heap: %u bytes, HAL tick: %lu\r\n",
                     counter++, xPortGetFreeHeapSize(), HAL_GetTick());

        vTaskDelay(pdMS_TO_TICKS(3000));
    }
}

/**
 * @brief 系统时钟配置
 * @details HSE: 8MHz
 *          PLL1: 8MHz / 1 * 120 / 2 = 480MHz SYSCLK
 *          PLL1_Q: 8MHz / 1 * 120 / 4 = 240MHz SDMMC时钟
 *          PLL3: 8MHz / 2 * 48 / 4 = 48MHz USB时钟
 *          HCLK: 240MHz (SYSCLK / 2)
 *          APB1/2/3/4: 120MHz (HCLK / 2)
 */
void SystemClock_Config(void)
{
    RCC_OscInitTypeDef RCC_OscInitStruct = {0};
    RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
    RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0};

    /* 电源配置 */
    HAL_PWREx_ConfigSupply(PWR_LDO_SUPPLY);

    /* 电压缩放 - VOS0以获得最大性能 */
    __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE0);
    while(!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY)) {}

    /* 配置HSE和PLL1 */
    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
    RCC_OscInitStruct.HSEState = RCC_HSE_ON;
    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
    RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
    RCC_OscInitStruct.PLL.PLLM = 1;
    RCC_OscInitStruct.PLL.PLLN = 120;
    RCC_OscInitStruct.PLL.PLLP = 2;
    RCC_OscInitStruct.PLL.PLLQ = 4;
    RCC_OscInitStruct.PLL.PLLR = 2;
    RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_3;
    RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE;
    RCC_OscInitStruct.PLL.PLLFRACN = 0;

    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
    {
        Error_Handler();
    }

    /* 配置PLL2用于SPI (80MHz)，PLL3用于USB，配置SDMMC时钟 */
    PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_USB | RCC_PERIPHCLK_SDMMC | RCC_PERIPHCLK_SPI123;
    PeriphClkInitStruct.UsbClockSelection = RCC_USBCLKSOURCE_PLL3;
    PeriphClkInitStruct.SdmmcClockSelection = RCC_SDMMCCLKSOURCE_PLL;  /* 使用PLL1_Q */
    PeriphClkInitStruct.Spi123ClockSelection = RCC_SPI123CLKSOURCE_PLL2; /* SPI2/3使用PLL2_P */

    /* PLL2配置：VCO = (8MHz / 2) * 40 = 160MHz, PLL2_P = 160MHz / 2 = 80MHz
     * 这样SPI可以配置为80MHz/8 = 10MHz，与PX4的BMI088和BMI270配置一致
     */
    PeriphClkInitStruct.PLL2.PLL2M = 2;
    PeriphClkInitStruct.PLL2.PLL2N = 40;
    PeriphClkInitStruct.PLL2.PLL2P = 2;
    PeriphClkInitStruct.PLL2.PLL2Q = 2;
    PeriphClkInitStruct.PLL2.PLL2R = 2;
    PeriphClkInitStruct.PLL2.PLL2RGE = RCC_PLL2VCIRANGE_2;
    PeriphClkInitStruct.PLL2.PLL2VCOSEL = RCC_PLL2VCOWIDE;
    PeriphClkInitStruct.PLL2.PLL2FRACN = 0;

    /* PLL3配置：用于USB */
    PeriphClkInitStruct.PLL3.PLL3M = 2;
    PeriphClkInitStruct.PLL3.PLL3N = 48;
    PeriphClkInitStruct.PLL3.PLL3P = 2;
    PeriphClkInitStruct.PLL3.PLL3Q = 4;
    PeriphClkInitStruct.PLL3.PLL3R = 2;
    PeriphClkInitStruct.PLL3.PLL3RGE = RCC_PLL3VCIRANGE_2;
    PeriphClkInitStruct.PLL3.PLL3VCOSEL = RCC_PLL3VCOWIDE;
    PeriphClkInitStruct.PLL3.PLL3FRACN = 0;

    if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
    {
        Error_Handler();
    }

    /* 配置系统时钟 */
    RCC_ClkInitStruct.ClockType = (RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_HCLK |
                                   RCC_CLOCKTYPE_D1PCLK1 | RCC_CLOCKTYPE_PCLK1 |
                                   RCC_CLOCKTYPE_PCLK2 | RCC_CLOCKTYPE_D3PCLK1);
    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
    RCC_ClkInitStruct.SYSCLKDivider = RCC_SYSCLK_DIV1;
    RCC_ClkInitStruct.AHBCLKDivider = RCC_HCLK_DIV2;
    RCC_ClkInitStruct.APB3CLKDivider = RCC_APB3_DIV2;
    RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV2;
    RCC_ClkInitStruct.APB2CLKDivider = RCC_APB2_DIV2;
    RCC_ClkInitStruct.APB4CLKDivider = RCC_APB4_DIV2;

    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
    {
        Error_Handler();
    }
}

/**
 * @brief GPIO初始化
 */
static void GPIO_Init(void)
{
    GPIO_InitTypeDef GPIO_InitStruct = {0};

    /* 使能GPIO时钟 */
    __HAL_RCC_GPIOE_CLK_ENABLE();
    __HAL_RCC_GPIOC_CLK_ENABLE();
    __HAL_RCC_GPIOB_CLK_ENABLE();

    /* 初始时所有LED关闭 */
    HAL_GPIO_WritePin(GPIOE, GPIO_PIN_2 | GPIO_PIN_3 | GPIO_PIN_4, GPIO_PIN_SET);

    /* 配置LED引脚 */
    GPIO_InitStruct.Pin = GPIO_PIN_2 | GPIO_PIN_3 | GPIO_PIN_4;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);

    /* 优先级3B：配置IMU数据就绪中断引脚 */
    /* BMI088加速度计INT引脚 - PC14 */
    GPIO_InitStruct.Pin = GPIO_PIN_14;
    GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;  /* 上升沿触发 */
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);

    /* BMI088陀螺仪INT引脚 - PC15 */
    GPIO_InitStruct.Pin = GPIO_PIN_15;
    GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;  /* 上升沿触发 */
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);

    /* BMI270 INT引脚 - PB7 */
    GPIO_InitStruct.Pin = GPIO_PIN_7;
    GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;  /* 上升沿触发 */
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

    /* 配置NVIC中断优先级 */
    /* EXTI15_10中断（BMI088加速度计PC14和陀螺仪PC15） */
    HAL_NVIC_SetPriority(EXTI15_10_IRQn, 5, 0);  /* 优先级5（高于FreeRTOS任务） */
    HAL_NVIC_EnableIRQ(EXTI15_10_IRQn);

    /* EXTI9_5中断（BMI270 PB7） */
    HAL_NVIC_SetPriority(EXTI9_5_IRQn, 5, 0);  /* 优先级5（高于FreeRTOS任务） */
    HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
}

/**
 * @brief USB设备初始化
 */
static void USB_Init(void)
{
    extern USBD_DescriptorsTypeDef FS_Desc;
    extern USBD_CDC_ItfTypeDef USBD_Interface_fops_FS;

    if (USBD_Init(&hUsbDeviceFS, &FS_Desc, DEVICE_FS) != USBD_OK)
    {
        Error_Handler();
    }

    if (USBD_RegisterClass(&hUsbDeviceFS, &USBD_CDC) != USBD_OK)
    {
        Error_Handler();
    }

    if (USBD_CDC_RegisterInterface(&hUsbDeviceFS, &USBD_Interface_fops_FS) != USBD_OK)
    {
        Error_Handler();
    }

    if (USBD_Start(&hUsbDeviceFS) != USBD_OK)
    {
        Error_Handler();
    }
}

/**
 * @brief 通过USB CDC进行调试输出
 */
/* 静态发送缓冲区（避免局部变量被销毁） */
#define USB_TX_BUFFER_COUNT 4
static char usb_tx_buffers[USB_TX_BUFFER_COUNT][256];
static uint8_t usb_tx_buffer_index = 0;

void debug_printf(const char *format, ...)
{
    va_list args;
    char *buffer;
    int len;

    /* 获取下一个可用缓冲区 */
    if (xUSBMutex != NULL && xTaskGetSchedulerState() == taskSCHEDULER_RUNNING)
    {
        if (xSemaphoreTake(xUSBMutex, pdMS_TO_TICKS(500)) == pdTRUE)
        {
            buffer = usb_tx_buffers[usb_tx_buffer_index];
            usb_tx_buffer_index = (usb_tx_buffer_index + 1) % USB_TX_BUFFER_COUNT;

            va_start(args, format);
            len = vsnprintf(buffer, 256, format, args);
            va_end(args);

            if (len > 0 && len < 256)
            {
                uint8_t result;
                int retry = 0;

                /* 重试发送，直到成功或超时 */
                while (retry < 50)
                {
                    result = CDC_Transmit_FS((uint8_t*)buffer, len);
                    if (result == USBD_OK)
                    {
                        /* 等待传输完成 */
                        vTaskDelay(pdMS_TO_TICKS(20));
                        break;
                    }
                    vTaskDelay(pdMS_TO_TICKS(10));
                    retry++;
                }
            }

            xSemaphoreGive(xUSBMutex);
        }
    }
    else
    {
        /* 调度器启动前，使用第一个缓冲区 */
        buffer = usb_tx_buffers[0];

        va_start(args, format);
        len = vsnprintf(buffer, 256, format, args);
        va_end(args);

        if (len > 0 && len < 256)
        {
            CDC_Transmit_FS((uint8_t*)buffer, len);
        }
    }
}

/**
 * @brief 错误处理函数
 */
void Error_Handler(void)
{
    __disable_irq();
    while (1)
    {
        /* 错误时红色LED闪烁 */
        HAL_GPIO_TogglePin(GPIOE, GPIO_PIN_3);
        for(volatile int i = 0; i < 1000000; i++);
    }
}

/**
 * @brief FreeRTOS空闲任务内存分配
 */
void vApplicationGetIdleTaskMemory(StaticTask_t **ppxIdleTaskTCBBuffer,
                                    StackType_t **ppxIdleTaskStackBuffer,
                                    uint32_t *pulIdleTaskStackSize)
{
    static StaticTask_t xIdleTaskTCB;
    static StackType_t uxIdleTaskStack[configMINIMAL_STACK_SIZE];

    *ppxIdleTaskTCBBuffer = &xIdleTaskTCB;
    *ppxIdleTaskStackBuffer = uxIdleTaskStack;
    *pulIdleTaskStackSize = configMINIMAL_STACK_SIZE;
}

/**
 * @brief FreeRTOS定时器任务内存分配
 */
void vApplicationGetTimerTaskMemory(StaticTask_t **ppxTimerTaskTCBBuffer,
                                     StackType_t **ppxTimerTaskStackBuffer,
                                     uint32_t *pulTimerTaskStackSize)
{
    static StaticTask_t xTimerTaskTCB;
    static StackType_t uxTimerTaskStack[configTIMER_TASK_STACK_DEPTH];

    *ppxTimerTaskTCBBuffer = &xTimerTaskTCB;
    *ppxTimerTaskStackBuffer = uxTimerTaskStack;
    *pulTimerTaskStackSize = configTIMER_TASK_STACK_DEPTH;
}

/**
 * @brief 栈溢出钩子函数
 */
void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName)
{
    (void)xTask;
    (void)pcTaskName;

    /* 栈溢出时所有LED快速闪烁 */
    while(1)
    {
        HAL_GPIO_TogglePin(GPIOE, GPIO_PIN_2 | GPIO_PIN_3 | GPIO_PIN_4);
        for(volatile int i = 0; i < 500000; i++);
    }
}

/**
 * @brief 内存分配失败钩子函数
 */
void vApplicationMallocFailedHook(void)
{
    /* 内存分配失败时蓝色LED快速闪烁 */
    while(1)
    {
        HAL_GPIO_TogglePin(GPIOE, GPIO_PIN_4);
        for(volatile int i = 0; i < 500000; i++);
    }
}

/**
 * @brief SD卡初始化
 */
static void SD_Init(void)
{
    FRESULT res;
    extern SD_HandleTypeDef hsd1;

    debug_printf("Linking SD card driver...\r\n");

    /* 链接SD卡驱动 */
    if (FATFS_LinkDriver(&SD_Driver, SDPath) == 0)
    {
        debug_printf("SD driver linked, mounting filesystem...\r\n");

        /* 挂载文件系统 */
        res = f_mount(&SDFatFS, (TCHAR const*)SDPath, 1);
        if (res == FR_OK)
        {
            sd_mounted = 1;
            debug_printf("SD card mounted successfully\r\n");

            /* 显示SD卡信息 */
            HAL_SD_CardInfoTypeDef cardinfo;
            if (HAL_SD_GetCardInfo(&hsd1, &cardinfo) == HAL_OK)
            {
                debug_printf("SD Card Info:\r\n");
                debug_printf("  Card Type: %lu\r\n", cardinfo.CardType);
                debug_printf("  Card Version: %lu\r\n", cardinfo.CardVersion);
                debug_printf("  Class: %lu\r\n", cardinfo.Class);
                debug_printf("  Capacity: %lu MB\r\n",
                            (uint32_t)(((uint64_t)cardinfo.BlockNbr * cardinfo.BlockSize) / (1024 * 1024)));
                debug_printf("  Block Size: %lu bytes\r\n", cardinfo.BlockSize);
            }
        }
        else
        {
            debug_printf("Failed to mount SD card: %d\r\n", res);
            debug_printf("SD card error code: 0x%08lX\r\n", hsd1.ErrorCode);
        }
    }
    else
    {
        debug_printf("Failed to link SD card driver\r\n");
    }
}

/**
 * @brief 写入文件
 * @param filename: 文件名
 * @param content: 文件内容
 */
static void SD_WriteFile(const char *filename, const char *content)
{
    FIL file;
    FRESULT res;
    UINT bytesWritten;
    char path[64];

    if (!sd_mounted)
    {
        debug_printf("Error: SD card not mounted\r\n");
        return;
    }

    /* 构建完整路径 */
    snprintf(path, sizeof(path), "%s%s", SDPath, filename);

    /* 打开文件 */
    res = f_open(&file, path, FA_CREATE_ALWAYS | FA_WRITE);
    if (res != FR_OK)
    {
        debug_printf("Failed to open file: %d\r\n", res);
        return;
    }

    /* 写入内容 */
    res = f_write(&file, content, strlen(content), &bytesWritten);
    if (res != FR_OK)
    {
        debug_printf("Failed to write file: %d\r\n", res);
    }
    else
    {
        debug_printf("Wrote %u bytes to %s\r\n", bytesWritten, filename);
    }

    /* 关闭文件 */
    f_close(&file);
}

/**
 * @brief 读取文件
 * @param filename: 文件名
 */
static void SD_ReadFile(const char *filename)
{
    FIL file;
    FRESULT res;
    char path[64];
    char buffer[256];

    if (!sd_mounted)
    {
        debug_printf("Error: SD card not mounted\r\n");
        return;
    }

    /* 构建完整路径 */
    snprintf(path, sizeof(path), "%s%s", SDPath, filename);

    /* 打开文件 */
    res = f_open(&file, path, FA_READ);
    if (res != FR_OK)
    {
        debug_printf("Failed to open file: %d\r\n", res);
        return;
    }

    debug_printf("File content (%s):\r\n", filename);
    debug_printf("----------------------------------------\r\n");

    /* 读取并显示内容 */
    while (f_gets(buffer, sizeof(buffer), &file))
    {
        debug_printf("%s", buffer);
    }

    debug_printf("\r\n----------------------------------------\r\n");
    debug_printf("File size: %lu bytes\r\n", f_size(&file));

    /* 关闭文件 */
    f_close(&file);
}

/**
 * @brief 列出文件
 */
static void SD_ListFiles(void)
{
    DIR dir;
    FILINFO fno;
    FRESULT res;
    uint32_t totalFiles = 0;
    uint32_t totalSize = 0;

    if (!sd_mounted)
    {
        debug_printf("Error: SD card not mounted\r\n");
        return;
    }

    /* 打开根目录 */
    res = f_opendir(&dir, SDPath);
    if (res != FR_OK)
    {
        debug_printf("Failed to open directory: %d\r\n", res);
        return;
    }

    debug_printf("File list:\r\n");
    debug_printf("----------------------------------------\r\n");

    /* 读取目录内容 */
    while (1)
    {
        res = f_readdir(&dir, &fno);
        if (res != FR_OK || fno.fname[0] == 0)
            break;

        if (!(fno.fattrib & AM_DIR))
        {
            debug_printf("%-20s %10lu bytes\r\n", fno.fname, fno.fsize);
            totalFiles++;
            totalSize += fno.fsize;
        }
    }

    debug_printf("----------------------------------------\r\n");
    debug_printf("Total: %lu files, %lu bytes\r\n", totalFiles, totalSize);

    f_closedir(&dir);
}

/**
 * @brief 删除文件
 * @param filename: 文件名
 */
static void SD_DeleteFile(const char *filename)
{
    FRESULT res;
    char path[64];

    if (!sd_mounted)
    {
        debug_printf("Error: SD card not mounted\r\n");
        return;
    }

    /* 构建完整路径 */
    snprintf(path, sizeof(path), "%s%s", SDPath, filename);

    /* 删除文件 */
    res = f_unlink(path);
    if (res == FR_OK)
    {
        debug_printf("Deleted file: %s\r\n", filename);
    }
    else
    {
        debug_printf("Failed to delete file: %d\r\n", res);
    }
}

/**
 * @brief 显示SD卡信息
 */
static void SD_ShowInfo(void)
{
    FATFS *fs;
    DWORD fre_clust, fre_sect, tot_sect;
    FRESULT res;
    HAL_SD_CardInfoTypeDef cardInfo;

    debug_printf("\r\nSD Card Status:\r\n");
    debug_printf("----------------------------------------\r\n");

    if (!sd_mounted)
    {
        debug_printf("SD card not mounted\r\n");
        debug_printf("Possible reasons:\r\n");
        debug_printf("  1. SD card not inserted\r\n");
        debug_printf("  2. SD card format incorrect (need FAT32)\r\n");
        debug_printf("  3. SD card hardware connection problem\r\n");
        debug_printf("----------------------------------------\r\n");
        return;
    }

    debug_printf("SD card mounted OK\r\n");

    /* 获取SD卡硬件信息 */
    HAL_SD_GetCardInfo(&hsd1, &cardInfo);

    debug_printf("\r\nSD卡信息:\r\n");
    debug_printf("----------------------------------------\r\n");
    debug_printf("卡类型: ");
    switch (cardInfo.CardType)
    {
    case CARD_SDSC:
        debug_printf("SDSC (Standard Capacity)\r\n");
        break;
    case CARD_SDHC_SDXC:
        debug_printf("SDHC/SDXC (High/Extended Capacity)\r\n");
        break;
    default:
        debug_printf("未知\r\n");
        break;
    }

    debug_printf("卡版本: %u.%u\r\n",
                 (cardInfo.CardVersion >> 4) & 0x0F,
                 cardInfo.CardVersion & 0x0F);
    debug_printf("块大小: %lu 字节\r\n", cardInfo.BlockSize);
    debug_printf("块数量: %lu\r\n", cardInfo.BlockNbr);
    debug_printf("总容量: %lu MB\r\n",
                 (cardInfo.BlockNbr * cardInfo.BlockSize) / (1024 * 1024));

    /* 获取文件系统信息 */
    res = f_getfree(SDPath, &fre_clust, &fs);
    if (res == FR_OK)
    {
        tot_sect = (fs->n_fatent - 2) * fs->csize;
        fre_sect = fre_clust * fs->csize;

        debug_printf("\r\n文件系统信息:\r\n");
        debug_printf("总空间: %lu KB\r\n", tot_sect / 2);
        debug_printf("可用空间: %lu KB\r\n", fre_sect / 2);
        debug_printf("已用空间: %lu KB\r\n", (tot_sect - fre_sect) / 2);
    }

    debug_printf("----------------------------------------\r\n");
}

/**
 * @brief USB命令处理
 * @param cmd: 命令字符串
 */
static void USB_ProcessCommand(char *cmd)
{
    char *token;
    char *filename;
    char *content;

    /* 去除末尾的换行符 */
    size_t len = strlen(cmd);
    while (len > 0 && (cmd[len-1] == '\r' || cmd[len-1] == '\n'))
    {
        cmd[--len] = '\0';
    }

    if (len == 0)
        return;

    debug_printf("\r\n命令: %s\r\n", cmd);

    /* 解析命令 */
    token = strtok(cmd, " ");
    if (token == NULL)
    {
        return;
    }

    if (strcmp(token, "write") == 0)
    {
        /* write <filename> <content> */
        filename = strtok(NULL, " ");
        content = strtok(NULL, "");  /* 获取剩余所有内容 */

        if (filename && content)
        {
            SD_WriteFile(filename, content);
        }
        else
        {
            debug_printf("用法: write <filename> <content>\r\n");
        }
    }
    else if (strcmp(token, "read") == 0)
    {
        /* read <filename> */
        filename = strtok(NULL, " ");

        if (filename)
        {
            SD_ReadFile(filename);
        }
        else
        {
            debug_printf("用法: read <filename>\r\n");
        }
    }
    else if (strcmp(token, "list") == 0)
    {
        /* list */
        SD_ListFiles();
    }
    else if (strcmp(token, "delete") == 0)
    {
        /* delete <filename> */
        filename = strtok(NULL, " ");

        if (filename)
        {
            SD_DeleteFile(filename);
        }
        else
        {
            debug_printf("用法: delete <filename>\r\n");
        }
    }
    else if (strcmp(token, "info") == 0)
    {
        /* info */
        debug_printf("Calling SD_ShowInfo()...\r\n");
        SD_ShowInfo();
        debug_printf("SD_ShowInfo() returned\r\n");
    }
    else if (strcmp(token, "help") == 0)
    {
        /* help */
        debug_printf("\r\nAvailable commands:\r\n");
        debug_printf("  write <filename> <content> - Write file\r\n");
        debug_printf("  read <filename>            - Read file\r\n");
        debug_printf("  list                       - List files\r\n");
        debug_printf("  delete <filename>          - Delete file\r\n");
        debug_printf("  info                       - Show SD card info\r\n");
        debug_printf("  help                       - Show help\r\n");
    }
    else
    {
        debug_printf("Unknown command: '%s' (type 'help' for help)\r\n", token);
    }
}

/**
 * @brief USB接收回调函数 (从usbd_cdc_if.c调用)
 * @param Buf: 接收到的数据
 * @param Len: 数据长度
 */
void USB_ReceiveCallback(uint8_t* Buf, uint32_t Len)
{
    /* 注意：这个函数在USB中断上下文中执行，不能调用debug_printf()或任何阻塞函数 */

    /* 将接收到的数据添加到缓冲区 */
    for (uint32_t i = 0; i < Len; i++)
    {
        if (Buf[i] == '\n' || Buf[i] == '\r')
        {
            /* 遇到换行符，准备处理命令 */
            if (usb_rx_len > 0 && !usb_cmd_ready)
            {
                usb_rx_buffer[usb_rx_len] = '\0';

                /* 复制到命令缓冲区 */
                for (uint16_t j = 0; j <= usb_rx_len; j++)
                {
                    usb_cmd_buffer[j] = usb_rx_buffer[j];
                }

                /* 设置命令就绪标志 */
                usb_cmd_ready = 1;
                usb_rx_len = 0;
            }
        }
        else if (usb_rx_len < USB_RX_BUFFER_SIZE - 1)
        {
            /* 添加字符到缓冲区 */
            usb_rx_buffer[usb_rx_len++] = Buf[i];
        }
        else
        {
            /* 缓冲区满，清空 */
            usb_rx_len = 0;
        }
    }
}

/**
 * @brief SPI初始化
 */
static void SPI_Init_All(void)
{
    if (BSP_SPI_Init() != 0)
    {
        debug_printf("ERROR: SPI initialization failed!\r\n");
        Error_Handler();
    }
    debug_printf("SPI2/SPI3 initialized successfully\r\n");
}

/* ========== 优先级3B：数据就绪中断支持 ========== */

/* 数据就绪标志（从中断设置，任务清除） */
static volatile uint8_t bmi088_accel_data_ready = 0;
static volatile uint8_t bmi088_gyro_data_ready = 0;
static volatile uint8_t bmi270_data_ready = 0;

/* 数据就绪中断计数器（用于统计） */
static volatile uint32_t bmi088_accel_irq_count = 0;
static volatile uint32_t bmi088_gyro_irq_count = 0;
static volatile uint32_t bmi270_irq_count = 0;

/**
 * @brief GPIO EXTI中断回调函数
 *
 * 由HAL_GPIO_EXTI_IRQHandler调用，处理数据就绪中断
 *
 * @param GPIO_Pin: 触发中断的GPIO引脚
 */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;

    switch (GPIO_Pin)
    {
    case GPIO_PIN_14:  /* BMI088加速度计数据就绪 - PC14 */
        bmi088_accel_data_ready = 1;
        bmi088_accel_irq_count++;
        /* 通知IMU任务有新数据 */
        if (xIMUTaskHandle != NULL) {
            vTaskNotifyGiveFromISR(xIMUTaskHandle, &xHigherPriorityTaskWoken);
        }
        break;

    case GPIO_PIN_15:  /* BMI088陀螺仪数据就绪 - PC15 */
        bmi088_gyro_data_ready = 1;
        bmi088_gyro_irq_count++;
        /* 通知IMU任务有新数据 */
        if (xIMUTaskHandle != NULL) {
            vTaskNotifyGiveFromISR(xIMUTaskHandle, &xHigherPriorityTaskWoken);
        }
        break;

    case GPIO_PIN_7:   /* BMI270数据就绪 - PB7 */
        bmi270_data_ready = 1;
        bmi270_irq_count++;
        /* 通知IMU任务有新数据 */
        if (xIMUTaskHandle != NULL) {
            vTaskNotifyGiveFromISR(xIMUTaskHandle, &xHigherPriorityTaskWoken);
        }
        break;

    default:
        break;
    }

    /* 如果需要，执行上下文切换 */
    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}

/**
 * @brief IMU任务 - 读取BMI088和BMI270数据（FIFO模式）
 */
static void IMU_Task(void *pvParameters)
{
    (void)pvParameters;
    BMI088_FIFO_AccelData_t bmi088_accel_fifo;
    BMI088_FIFO_GyroData_t bmi088_gyro_fifo;
    BMI270_FIFO_Data_t bmi270_fifo;  /* 使用新的组合数据结构 */
    BMI088_Status_t status;
    uint32_t counter = 0;
    uint8_t bmi088_initialized = 0;
    uint8_t bmi270_initialized = 0;
    char msg[512];

    /* 统计信息 */
    uint32_t bmi088_accel_zero_count = 0;  /* BMI088加速度计FIFO为空的次数 */
    uint32_t bmi088_gyro_zero_count = 0;   /* BMI088陀螺仪FIFO为空的次数 */
    uint32_t bmi270_accel_zero_count = 0;  /* BMI270加速度计FIFO为空的次数 */
    uint32_t bmi270_gyro_zero_count = 0;   /* BMI270陀螺仪FIFO为空的次数 */
    uint32_t bmi270_both_zero_count = 0;   /* BMI270两个都为空的次数 */
    uint32_t bmi088_total_reads = 0;       /* BMI088总读取次数 */
    uint32_t bmi270_total_reads = 0;       /* BMI270总读取次数 */

    /* 优先级2：重要改进功能的计数器 */
    uint32_t register_check_counter = 0;   /* 寄存器验证计数器（每100ms检查一次） */
    uint32_t error_check_counter = 0;      /* 错误检测计数器（每100ms检查一次） */
    uint32_t temperature_counter = 0;      /* 温度读取计数器（每1秒读取一次） */

    /* 温度数据 */
    float bmi088_temperature = 0.0f;
    float bmi270_temperature = 0.0f;

    /* 等待1秒让系统稳定，特别是USB */
    vTaskDelay(pdMS_TO_TICKS(1000));

    /* 使用CDC_Transmit_FS直接发送，带重试机制 */
    snprintf(msg, sizeof(msg), "\r\n=== IMU Task started ===\r\n");
    for (int retry = 0; retry < 10; retry++)
    {
        if (CDC_Transmit_FS((uint8_t*)msg, strlen(msg)) == USBD_OK)
            break;
        vTaskDelay(pdMS_TO_TICKS(100));
    }
    vTaskDelay(pdMS_TO_TICKS(100));

    /* 初始化BMI088（在FreeRTOS任务中，HAL_Delay可以正常工作） */
    snprintf(msg, sizeof(msg), "Initializing BMI088...\r\n");
    for (int retry = 0; retry < 10; retry++)
    {
        if (CDC_Transmit_FS((uint8_t*)msg, strlen(msg)) == USBD_OK)
            break;
        vTaskDelay(pdMS_TO_TICKS(100));
    }
    vTaskDelay(pdMS_TO_TICKS(100));

    if (xSemaphoreTake(xSPIMutex, pdMS_TO_TICKS(100)) == pdTRUE)
    {
        /* 先读取芯片ID看看SPI通信是否正常 */
        uint8_t accel_id = 0, gyro_id = 0;

        /* 尝试读取加速度计芯片ID */
        BSP_SPI_BMI088_CS(BMI088_ACCEL, 1);
        HAL_Delay(1);
        uint8_t reg = BMI088_ACC_CHIP_ID | 0x80;  /* 读操作 */
        uint8_t dummy = 0;
        BSP_SPI_Transfer(SPI_PORT_2, &reg, NULL, 1);
        BSP_SPI_Transfer(SPI_PORT_2, &dummy, &dummy, 1);  /* dummy byte */
        BSP_SPI_Transfer(SPI_PORT_2, &dummy, &accel_id, 1);  /* actual data */
        BSP_SPI_BMI088_CS(BMI088_ACCEL, 0);
        HAL_Delay(1);

        snprintf(msg, sizeof(msg), "Accel Chip ID read: 0x%02X (expected: 0x1E)\r\n", accel_id);
        CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
        vTaskDelay(pdMS_TO_TICKS(100));

        /* 尝试读取陀螺仪芯片ID */
        BSP_SPI_BMI088_CS(BMI088_GYRO, 1);
        HAL_Delay(1);
        reg = BMI088_GYRO_CHIP_ID | 0x80;  /* 读操作 */
        BSP_SPI_Transfer(SPI_PORT_2, &reg, NULL, 1);
        BSP_SPI_Transfer(SPI_PORT_2, &dummy, &gyro_id, 1);
        BSP_SPI_BMI088_CS(BMI088_GYRO, 0);
        HAL_Delay(1);

        snprintf(msg, sizeof(msg), "Gyro Chip ID read: 0x%02X (expected: 0x0F)\r\n", gyro_id);
        CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
        vTaskDelay(pdMS_TO_TICKS(100));

        /* 尝试正式初始化 */
        if (BMI088_Init() == 0)
        {
            /* 获取初始化状态 */
            BMI088_GetStatus(&status);

            snprintf(msg, sizeof(msg), "BMI088 initialized successfully\r\n");
            CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
            vTaskDelay(pdMS_TO_TICKS(100));

            snprintf(msg, sizeof(msg), "  Accel Chip ID: 0x%02X (expected: 0x1E)\r\n", status.accel_chip_id);
            CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
            vTaskDelay(pdMS_TO_TICKS(100));

            snprintf(msg, sizeof(msg), "  Gyro Chip ID:  0x%02X (expected: 0x0F)\r\n", status.gyro_chip_id);
            CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
            vTaskDelay(pdMS_TO_TICKS(100));

            /* 初始化FIFO模式 */
            snprintf(msg, sizeof(msg), "Initializing FIFO mode...\r\n");
            CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
            vTaskDelay(pdMS_TO_TICKS(100));

            /* 配置FIFO水印：完全按照PX4的配置
             * 加速度计：4样本（28字节），400Hz中断频率
             * 陀螺仪：5样本，400Hz中断频率
             * 参考PX4: ConfigureSampleRate(IMU_GYRO_RATEMAX=400Hz)
             */
            if (BMI088_FIFO_Init(4, 5) == 0)
            {
                snprintf(msg, sizeof(msg), "FIFO mode initialized successfully!\r\n");
                snprintf(msg + strlen(msg), sizeof(msg) - strlen(msg), "  Accel watermark: 4 samples (28 bytes) - 400Hz INT\r\n");
                snprintf(msg + strlen(msg), sizeof(msg) - strlen(msg), "  Gyro watermark: 5 samples - 400Hz INT\r\n");
                CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
                vTaskDelay(pdMS_TO_TICKS(100));
                bmi088_initialized = 1;
            }
            else
            {
                snprintf(msg, sizeof(msg), "ERROR: FIFO initialization failed!\r\n");
                CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
                vTaskDelay(pdMS_TO_TICKS(100));
            }
        }
        else
        {
            /* 获取状态看看哪个失败了 */
            BMI088_GetStatus(&status);

            snprintf(msg, sizeof(msg), "ERROR: BMI088 initialization failed!\r\n");
            CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
            vTaskDelay(pdMS_TO_TICKS(100));

            snprintf(msg, sizeof(msg), "  Accel init: %s (ID: 0x%02X)\r\n",
                     status.accel_initialized ? "OK" : "FAILED", status.accel_chip_id);
            CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
            vTaskDelay(pdMS_TO_TICKS(100));

            snprintf(msg, sizeof(msg), "  Gyro init: %s (ID: 0x%02X)\r\n",
                     status.gyro_initialized ? "OK" : "FAILED", status.gyro_chip_id);
            CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
            vTaskDelay(pdMS_TO_TICKS(100));
        }

        /* 初始化BMI270（SPI3） */
        snprintf(msg, sizeof(msg), "\r\nInitializing BMI270...\r\n");
        CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
        vTaskDelay(pdMS_TO_TICKS(100));

        /* 运行详细调试 */
        char debug_buf[1024];
        int debug_len = BMI270_DebugInfo(debug_buf, sizeof(debug_buf));
        if (debug_len > 0) {
            /* 分段发送调试信息 */
            int offset = 0;
            while (offset < debug_len) {
                int chunk_size = (debug_len - offset) > 256 ? 256 : (debug_len - offset);
                CDC_Transmit_FS((uint8_t*)(debug_buf + offset), chunk_size);
                offset += chunk_size;
                vTaskDelay(pdMS_TO_TICKS(50));
            }
        }

        /* 先读取Chip ID */
        uint8_t bmi270_chip_id = BMI270_ReadChipID();
        snprintf(msg, sizeof(msg), "BMI270 Chip ID read: 0x%02X (expected: 0x24)\r\n", bmi270_chip_id);
        CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
        vTaskDelay(pdMS_TO_TICKS(100));

        int bmi270_init_result = BMI270_Init();
        if (bmi270_init_result == 0)
        {
            snprintf(msg, sizeof(msg), "BMI270 initialized successfully\r\n");
            CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
            vTaskDelay(pdMS_TO_TICKS(100));

            /* 初始化FIFO模式：完全按照PX4的配置
             * 水印：52字节（4样本 × 13字节/样本），400Hz中断频率
             * 参考PX4: ConfigureSampleRate(IMU_GYRO_RATEMAX=400Hz)
             */
            if (BMI270_FIFO_Init(52) == 0)  /* 52字节水印 = 4样本 */
            {
                snprintf(msg, sizeof(msg), "BMI270 FIFO mode initialized successfully!\r\n");
                snprintf(msg + strlen(msg), sizeof(msg) - strlen(msg), "  FIFO watermark: 52 bytes (4 samples) - 400Hz INT\r\n");
                CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
                vTaskDelay(pdMS_TO_TICKS(100));
                bmi270_initialized = 1;
            }
            else
            {
                snprintf(msg, sizeof(msg), "ERROR: BMI270 FIFO initialization failed!\r\n");
                CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
                vTaskDelay(pdMS_TO_TICKS(100));
            }
        }
        else
        {
            if (bmi270_init_result == -1) {
                snprintf(msg, sizeof(msg), "ERROR: BMI270 SPI read failed!\r\n");
            } else if (bmi270_init_result == -2) {
                snprintf(msg, sizeof(msg), "ERROR: BMI270 Chip ID mismatch! (got 0x%02X, expected 0x24)\r\n", bmi270_chip_id);
            } else if (bmi270_init_result == -3) {
                snprintf(msg, sizeof(msg), "ERROR: BMI270 config file upload failed!\r\n");
            } else if (bmi270_init_result == -4) {
                snprintf(msg, sizeof(msg), "ERROR: BMI270 INTERNAL_STATUS check failed! Microcode not loaded.\r\n");
            } else {
                snprintf(msg, sizeof(msg), "ERROR: BMI270 initialization failed! (code: %d)\r\n", bmi270_init_result);
            }
            CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
            vTaskDelay(pdMS_TO_TICKS(100));
        }

        xSemaphoreGive(xSPIMutex);
    }
    else
    {
        snprintf(msg, sizeof(msg), "ERROR: Failed to acquire SPI mutex!\r\n");
        CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
        vTaskDelay(pdMS_TO_TICKS(100));
    }

    /* 优先级3B：混合模式计数器 - 用于超时后的定时器轮询 */
    uint32_t bmi088_poll_counter = 0;
    uint32_t bmi270_poll_counter = 0;

    while (1)
    {
        /* 优先级3B：混合驱动模式 - 中断驱动 + 定时器备份
         * 参考PX4: 主要使用中断驱动，但保留定时器作为备份
         * 超时时间设置为3ms，超时后使用定时器轮询防止死锁
         */
        uint32_t notification = ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(3));

        if (bmi088_initialized || bmi270_initialized)
        {
            /* 获取SPI互斥量 */
            if (xSemaphoreTake(xSPIMutex, pdMS_TO_TICKS(10)) == pdTRUE)
            {
                int bmi088_accel_samples = 0, bmi088_gyro_samples = 0;
                int bmi270_accel_samples = 0, bmi270_gyro_samples = 0;

                /* 优先级3B：混合驱动FIFO读取
                 * 1. 优先使用中断驱动（当有中断通知时）
                 * 2. 备用定时器轮询（当超时时，防止中断丢失导致死锁）
                 * 参考PX4: 中断触发时立即读取FIFO，获得最低延迟
                 */

                /* 读取BMI088 FIFO - 混合驱动 */
                bmi088_poll_counter++;
                if (bmi088_initialized &&
                    ((notification > 0 && (bmi088_accel_data_ready || bmi088_gyro_data_ready)) ||  /* 中断驱动 */
                     (bmi088_poll_counter % 1 == 0)))  /* 定时器备份：每3ms轮询一次 */
                {
                    /* 清除数据就绪标志 */
                    bmi088_accel_data_ready = 0;
                    bmi088_gyro_data_ready = 0;

                    bmi088_accel_samples = BMI088_FIFO_ReadAccel(&bmi088_accel_fifo);
                    bmi088_gyro_samples = BMI088_FIFO_ReadGyro(&bmi088_gyro_fifo);

                    /* 统计FIFO为空的情况 */
                    if (bmi088_accel_samples == 0) bmi088_accel_zero_count++;
                    if (bmi088_gyro_samples == 0) bmi088_gyro_zero_count++;
                    bmi088_total_reads++;

                    /* 优先级3：失败检测和自动恢复 */
                    if (bmi088_accel_samples < 0 || bmi088_gyro_samples < 0) {
                        /* 读取失败，增加失败计数 */
                        BMI088_IncrementFailureCount();

                        /* 如果失败次数超过10次，自动复位传感器 */
                        if (BMI088_GetFailureCount() > 10) {
                            BMI088_Reset();
                            BMI088_ResetFailureCount();
                        }
                    } else if (bmi088_accel_samples > 0 || bmi088_gyro_samples > 0) {
                        /* 读取成功，重置失败计数 */
                        BMI088_ResetFailureCount();
                    }

                    /* 优先级3：FIFO溢出检测 */
                    int overflow = BMI088_FIFO_CheckOverflow();
                    if (overflow == 1) {
                        /* FIFO溢出，强制复位FIFO */
                        BMI088_FIFO_ForceReset();
                    }
                }

                /* 读取BMI270 FIFO - 混合驱动 */
                bmi270_poll_counter++;
                if (bmi270_initialized &&
                    ((notification > 0 && bmi270_data_ready) ||  /* 中断驱动 */
                     (bmi270_poll_counter % 1 == 0)))  /* 定时器备份：每3ms轮询一次 */
                {
                    /* 清除数据就绪标志 */
                    bmi270_data_ready = 0;

                    int read_result = BMI270_FIFO_Read(&bmi270_fifo);

                    if (read_result == 0)
                    {
                        bmi270_accel_samples = bmi270_fifo.accel.accel_count;
                        bmi270_gyro_samples = bmi270_fifo.gyro.gyro_count;

                        /* 读取成功，重置失败计数 */
                        if (bmi270_accel_samples > 0 || bmi270_gyro_samples > 0) {
                            BMI270_ResetFailureCount();
                        }
                    }
                    else
                    {
                        bmi270_accel_samples = 0;
                        bmi270_gyro_samples = 0;

                        /* 优先级3：失败检测和自动恢复 */
                        BMI270_IncrementFailureCount();

                        /* 如果失败次数超过10次，自动复位传感器 */
                        if (BMI270_GetFailureCount() > 10) {
                            BMI270_Reset();
                            BMI270_ResetFailureCount();
                        }
                    }

                    /* 统计FIFO为空的情况 */
                    if (bmi270_accel_samples == 0) bmi270_accel_zero_count++;
                    if (bmi270_gyro_samples == 0) bmi270_gyro_zero_count++;
                    if (bmi270_accel_samples == 0 && bmi270_gyro_samples == 0) bmi270_both_zero_count++;
                    bmi270_total_reads++;

                    /* 优先级3：FIFO溢出检测 */
                    int overflow = BMI270_FIFO_CheckOverflow();
                    if (overflow == 1) {
                        /* FIFO溢出，强制复位FIFO */
                        BMI270_FIFO_ForceReset();
                    }
                }

                /* 优先级2：温度读取 - 每1秒读取一次
                 * 注意：使用混合模式，计数器基于3ms周期
                 * 1秒 = 333次（1000ms / 3ms）
                 */
                temperature_counter++;
                if (temperature_counter >= 333)  /* 约1秒（333 * 3ms） */
                {
                    temperature_counter = 0;

                    /* 读取BMI088温度 */
                    if (bmi088_initialized)
                    {
                        BMI088_Accel_UpdateTemperature(&bmi088_temperature);
                    }

                    /* 读取BMI270温度 */
                    if (bmi270_initialized)
                    {
                        BMI270_UpdateTemperature(&bmi270_temperature);
                    }
                }

                /* 优先级2：错误寄存器检测 - 每100ms检查一次
                 * 约33次（100ms / 3ms）
                 */
                error_check_counter++;
                if (error_check_counter >= 33)  /* 约100ms */
                {
                    error_check_counter = 0;

                    /* 检查BMI270错误寄存器 */
                    if (bmi270_initialized)
                    {
                        BMI270_CheckErrorRegister();
                    }
                }

                /* 优先级2：寄存器配置验证 - 每100ms检查一次（轮询不同的寄存器） */
                register_check_counter++;
                if (register_check_counter >= 33)  /* 约100ms */
                {
                    register_check_counter = 0;

                    /* 注意：寄存器验证功能已实现，但暂时不启用，避免频繁SPI读取影响性能 */
                    /* 如需启用，取消下面的注释 */
                    /*
                    static uint8_t reg_check_index = 0;

                    // 验证BMI088寄存器（轮询）
                    if (bmi088_initialized && reg_check_index < 5)
                    {
                        // 这里可以添加寄存器验证逻辑
                        // BMI088_Accel_RegisterCheck(&g_bmi088_accel_register_config[reg_check_index]);
                    }

                    // 验证BMI270寄存器（轮询）
                    if (bmi270_initialized && reg_check_index < 7)
                    {
                        // 这里可以添加寄存器验证逻辑
                        // BMI270_RegisterCheck(&g_bmi270_register_config[reg_check_index]);
                    }

                    reg_check_index = (reg_check_index + 1) % 7;  // 轮询7个寄存器
                    */
                }

                /* 每333次（约1秒）输出一次统计信息和数据质量分析
                 * 注意：使用混合模式，基于3ms周期
                 */
                if (counter % 333 == 0)
                {
                    snprintf(msg, sizeof(msg), "\r\n========== IMU Statistics ==========\r\n");
                    CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
                    vTaskDelay(pdMS_TO_TICKS(20));

                    /* 输出BMI088统计 */
                    if (bmi088_initialized)
                    {
                        uint32_t accel_success_rate = (bmi088_total_reads > 0) ? ((bmi088_total_reads - bmi088_accel_zero_count) * 100 / bmi088_total_reads) : 0;
                        uint32_t gyro_success_rate = (bmi088_total_reads > 0) ? ((bmi088_total_reads - bmi088_gyro_zero_count) * 100 / bmi088_total_reads) : 0;

                        snprintf(msg, sizeof(msg), "[BMI088] Reads: %lu, Accel: %d samples (zero: %lu, rate: %lu%%)\r\n",
                                 bmi088_total_reads, bmi088_accel_samples, bmi088_accel_zero_count, accel_success_rate);
                        CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
                        vTaskDelay(pdMS_TO_TICKS(20));

                        snprintf(msg, sizeof(msg), "         Gyro:  %d samples (zero: %lu, rate: %lu%%)\r\n",
                                 bmi088_gyro_samples, bmi088_gyro_zero_count, gyro_success_rate);
                        CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
                        vTaskDelay(pdMS_TO_TICKS(20));

                        /* 输出第一个样本 */
                        if (bmi088_accel_samples > 0)
                        {
                            float ax = (float)bmi088_accel_fifo.accel_x[0] * BMI088_ACCEL_SCALE_24G;
                            float ay = (float)bmi088_accel_fifo.accel_y[0] * BMI088_ACCEL_SCALE_24G;
                            float az = (float)bmi088_accel_fifo.accel_z[0] * BMI088_ACCEL_SCALE_24G;

                            int ax_int = (int)(ax * 100);
                            int ay_int = (int)(ay * 100);
                            int az_int = (int)(az * 100);

                            snprintf(msg, sizeof(msg), "         ACC: X=%d.%02d Y=%d.%02d Z=%d.%02d m/s²\r\n",
                                     ax_int/100, abs(ax_int%100), ay_int/100, abs(ay_int%100), az_int/100, abs(az_int%100));
                            CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
                            vTaskDelay(pdMS_TO_TICKS(20));
                        }

                        if (bmi088_gyro_samples > 0)
                        {
                            float gx = (float)bmi088_gyro_fifo.gyro_x[0] * BMI088_GYRO_SCALE_2000DPS;
                            float gy = (float)bmi088_gyro_fifo.gyro_y[0] * BMI088_GYRO_SCALE_2000DPS;
                            float gz = (float)bmi088_gyro_fifo.gyro_z[0] * BMI088_GYRO_SCALE_2000DPS;

                            int gx_int = (int)(gx * 1000);
                            int gy_int = (int)(gy * 1000);
                            int gz_int = (int)(gz * 1000);

                            snprintf(msg, sizeof(msg), "         GYR: X=%d.%03d Y=%d.%03d Z=%d.%03d rad/s\r\n",
                                     gx_int/1000, abs(gx_int%1000), gy_int/1000, abs(gy_int%1000), gz_int/1000, abs(gz_int%1000));
                            CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
                            vTaskDelay(pdMS_TO_TICKS(20));
                        }

                        /* 输出BMI088温度 */
                        int temp_int = (int)(bmi088_temperature * 100);
                        snprintf(msg, sizeof(msg), "         TEMP: %d.%02d °C\r\n",
                                 temp_int/100, abs(temp_int%100));
                        CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
                        vTaskDelay(pdMS_TO_TICKS(20));

                        /* 输出BMI088错误统计 */
                        BMI088_ErrorStats_t bmi088_errors;
                        BMI088_GetErrorStats(&bmi088_errors);
                        if (bmi088_errors.accel_bad_register_count > 0 || bmi088_errors.accel_bad_transfer_count > 0 ||
                            bmi088_errors.gyro_bad_register_count > 0 || bmi088_errors.gyro_bad_transfer_count > 0)
                        {
                            snprintf(msg, sizeof(msg), "         ERRORS: Acc(reg:%lu,xfer:%lu) Gyr(reg:%lu,xfer:%lu)\r\n",
                                     bmi088_errors.accel_bad_register_count, bmi088_errors.accel_bad_transfer_count,
                                     bmi088_errors.gyro_bad_register_count, bmi088_errors.gyro_bad_transfer_count);
                            CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
                            vTaskDelay(pdMS_TO_TICKS(20));
                        }

                        /* 优先级3：输出失败计数和状态机状态 */
                        uint32_t failure_count = BMI088_GetFailureCount();
                        BMI088_State_t state = BMI088_GetState();
                        const char *state_str = (state == BMI088_STATE_RESET) ? "RESET" :
                                                (state == BMI088_STATE_WAIT_FOR_RESET) ? "WAIT_RESET" :
                                                (state == BMI088_STATE_CONFIGURE) ? "CONFIGURE" :
                                                (state == BMI088_STATE_FIFO_RESET) ? "FIFO_RESET" :
                                                (state == BMI088_STATE_FIFO_READ) ? "RUNNING" : "UNKNOWN";

                        if (failure_count > 0 || state != BMI088_STATE_FIFO_READ) {
                            snprintf(msg, sizeof(msg), "         STATE: %s, Failures: %lu\r\n",
                                     state_str, failure_count);
                            CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
                            vTaskDelay(pdMS_TO_TICKS(20));
                        }

                        /* 优先级3B：输出数据就绪中断计数 */
                        snprintf(msg, sizeof(msg), "         IRQ: Accel=%lu Gyro=%lu\r\n",
                                 bmi088_accel_irq_count, bmi088_gyro_irq_count);
                        CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
                        vTaskDelay(pdMS_TO_TICKS(20));
                    }

                    /* 输出BMI270统计 */
                    if (bmi270_initialized)
                    {
                        uint32_t accel_success_rate = (bmi270_total_reads > 0) ? ((bmi270_total_reads - bmi270_accel_zero_count) * 100 / bmi270_total_reads) : 0;
                        uint32_t gyro_success_rate = (bmi270_total_reads > 0) ? ((bmi270_total_reads - bmi270_gyro_zero_count) * 100 / bmi270_total_reads) : 0;
                        uint32_t both_success_rate = (bmi270_total_reads > 0) ? ((bmi270_total_reads - bmi270_both_zero_count) * 100 / bmi270_total_reads) : 0;

                        snprintf(msg, sizeof(msg), "[BMI270] Reads: %lu, Accel: %d samples (zero: %lu, rate: %lu%%)\r\n",
                                 bmi270_total_reads, bmi270_accel_samples, bmi270_accel_zero_count, accel_success_rate);
                        CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
                        vTaskDelay(pdMS_TO_TICKS(20));

                        snprintf(msg, sizeof(msg), "         Gyro:  %d samples (zero: %lu, rate: %lu%%)\r\n",
                                 bmi270_gyro_samples, bmi270_gyro_zero_count, gyro_success_rate);
                        CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
                        vTaskDelay(pdMS_TO_TICKS(20));

                        snprintf(msg, sizeof(msg), "         Both zero: %lu times (success rate: %lu%%)\r\n",
                                 bmi270_both_zero_count, both_success_rate);
                        CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
                        vTaskDelay(pdMS_TO_TICKS(20));

                        /* 输出第一个样本 */
                        if (bmi270_accel_samples > 0)
                        {
                            float ax = (float)bmi270_fifo.accel.accel_x[0] * BMI270_ACC_SCALE_16G;
                            float ay = (float)bmi270_fifo.accel.accel_y[0] * BMI270_ACC_SCALE_16G;
                            float az = (float)bmi270_fifo.accel.accel_z[0] * BMI270_ACC_SCALE_16G;

                            int ax_int = (int)(ax * 100);
                            int ay_int = (int)(ay * 100);
                            int az_int = (int)(az * 100);

                            snprintf(msg, sizeof(msg), "         ACC: X=%d.%02d Y=%d.%02d Z=%d.%02d m/s²\r\n",
                                     ax_int/100, abs(ax_int%100), ay_int/100, abs(ay_int%100), az_int/100, abs(az_int%100));
                            CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
                            vTaskDelay(pdMS_TO_TICKS(20));
                        }

                        if (bmi270_gyro_samples > 0)
                        {
                            float gx = (float)bmi270_fifo.gyro.gyro_x[0] * BMI270_GYR_SCALE_2000DPS;
                            float gy = (float)bmi270_fifo.gyro.gyro_y[0] * BMI270_GYR_SCALE_2000DPS;
                            float gz = (float)bmi270_fifo.gyro.gyro_z[0] * BMI270_GYR_SCALE_2000DPS;

                            int gx_int = (int)(gx * 1000);
                            int gy_int = (int)(gy * 1000);
                            int gz_int = (int)(gz * 1000);

                            snprintf(msg, sizeof(msg), "         GYR: X=%d.%03d Y=%d.%03d Z=%d.%03d rad/s\r\n",
                                     gx_int/1000, abs(gx_int%1000), gy_int/1000, abs(gy_int%1000), gz_int/1000, abs(gz_int%1000));
                            CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
                            vTaskDelay(pdMS_TO_TICKS(20));
                        }

                        /* 输出BMI270温度 */
                        int temp_int = (int)(bmi270_temperature * 100);
                        snprintf(msg, sizeof(msg), "         TEMP: %d.%02d °C\r\n",
                                 temp_int/100, abs(temp_int%100));
                        CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
                        vTaskDelay(pdMS_TO_TICKS(20));

                        /* 输出BMI270错误统计 */
                        BMI270_ErrorStats_t bmi270_errors;
                        BMI270_GetErrorStats(&bmi270_errors);
                        if (bmi270_errors.bad_register_count > 0 || bmi270_errors.bad_transfer_count > 0 ||
                            bmi270_errors.fifo_overflow_count > 0)
                        {
                            snprintf(msg, sizeof(msg), "         ERRORS: reg:%lu xfer:%lu fifo_ovf:%lu\r\n",
                                     bmi270_errors.bad_register_count, bmi270_errors.bad_transfer_count,
                                     bmi270_errors.fifo_overflow_count);
                            CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
                            vTaskDelay(pdMS_TO_TICKS(20));
                        }

                        /* 优先级3：输出失败计数和状态机状态 */
                        uint32_t failure_count = BMI270_GetFailureCount();
                        BMI270_State_t state = BMI270_GetState();
                        const char *state_str = (state == BMI270_STATE_RESET) ? "RESET" :
                                                (state == BMI270_STATE_WAIT_FOR_RESET) ? "WAIT_RESET" :
                                                (state == BMI270_STATE_MICROCODE_LOAD) ? "MICROCODE" :
                                                (state == BMI270_STATE_CONFIGURE) ? "CONFIGURE" :
                                                (state == BMI270_STATE_FIFO_READ) ? "RUNNING" : "UNKNOWN";

                        if (failure_count > 0 || state != BMI270_STATE_FIFO_READ) {
                            snprintf(msg, sizeof(msg), "         STATE: %s, Failures: %lu\r\n",
                                     state_str, failure_count);
                            CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
                            vTaskDelay(pdMS_TO_TICKS(20));
                        }

                        /* 优先级3B：输出数据就绪中断计数 */
                        snprintf(msg, sizeof(msg), "         IRQ: %lu\r\n", bmi270_irq_count);
                        CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
                        vTaskDelay(pdMS_TO_TICKS(20));
                    }

                    snprintf(msg, sizeof(msg), "====================================================\r\n");
                    CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
                    vTaskDelay(pdMS_TO_TICKS(20));
                }

                /* 计数器递增 */
                counter++;

                /* 释放SPI互斥量 */
                xSemaphoreGive(xSPIMutex);
            }
        }

        /* 优先级3B：混合驱动模式
         * 1. 主要使用中断驱动（在循环开始时等待中断通知，超时3ms）
         * 2. 备用定时器轮询（超时后每3ms轮询一次，防止中断丢失）
         * 这样既能获得中断驱动的低延迟，又能防止死锁
         * 参考PX4: 中断驱动 + 定时器备份的混合模式
         */
    }
}

